// #include "map_base.h"
#include <mc_map/map_base.h>

// 设置地图数据
void MapBase::setOccupancyGrid(const OccupancyGridPtr& occupancy_grid_ptr){
    mapOriginGridPtr_ = occupancy_grid_ptr;

    // 地图元数据信息
    resolution_ = occupancy_grid_ptr->info.resolution;
    width_ = occupancy_grid_ptr->info.width;
    height_ = occupancy_grid_ptr->info.height;
    origin_ = cv::Point2f(occupancy_grid_ptr->info.origin.position.x,
        occupancy_grid_ptr->info.origin.position.y);
    mapSize_ = width_ * height_;
    frameId_ = occupancy_grid_ptr->header.frame_id;
    
    // 转map
    cvMap_ = cv::Mat::zeros(height_, width_, CV_8UC1);
    cvMapBinary_ = cv::Mat::zeros(height_, width_, CV_8UC1);
    for (int h = 0; h < height_; h++){
        for (int w = 0; w < width_; w++){
            int8_t occupancy_value = occupancy_grid_ptr->data[h * width_ + w];
            //反转y轴坐标 (ros中原点在左下角，opencv中原点在左上角)
            int flipped_h = height_ - 1 - h; 

            // 填充cvMap_
            if(occupancy_value > 70){
                cvMap_.at<uchar>(flipped_h, w) = 0;
            }else if(occupancy_value < 30 && occupancy_value >= 0){
                cvMap_.at<uchar>(flipped_h, w) = 255;
            }
            else{
                cvMap_.at<uchar>(flipped_h, w) = 127;
            }

            // 填充cvMapBinary_
            if(occupancy_value < 30 && occupancy_value >= 0){
                cvMapBinary_.at<uchar>(flipped_h, w) = 255;
            }
            else{
                cvMapBinary_.at<uchar>(flipped_h, w) = 0;
            }
        }
    }

    LOG(INFO) << "Map Info:";
    LOG(INFO) << "--> Resolution: " << occupancy_grid_ptr->info.resolution << " meters per cell";
    LOG(INFO) << "--> Width: " << occupancy_grid_ptr->info.width 
            << " *  Height: " << occupancy_grid_ptr->info.height << ", Total grid cells: "
            << occupancy_grid_ptr->info.width * occupancy_grid_ptr->info.height;
    LOG(INFO) << "--> Origin (x, y, z): ("
              << occupancy_grid_ptr->info.origin.position.x << ", "
              << occupancy_grid_ptr->info.origin.position.y << ", "
              << occupancy_grid_ptr->info.origin.position.z << ")";
}


// 世界坐标转像素坐标
// template <typename T1, typename T2>
// T2 MapBase::WorldToPixel(const T1& point){
//     T2 pixel_point(
//         std::round((point.x - origin_.x) / resolution_), 
//         std::round((point.y - origin_.y) / resolution_));
//     return pixel_point;
// }

// 世界坐标转像素坐标
// template <typename T1,  typename T2>
// T2 MapBase::WorldToOpencvPixel(const T1& point){
//     T2 pixel_point(
//         std::round((point.x - origin_.x) / resolution_), 
//         std::round((point.y - origin_.y) / resolution_));
//     pixel_point.y = height_ - 1 - pixel_point.y; 
//     return pixel_point;
// }


// 像素坐标转世界坐标
// template <typename T1,  typename T2>
// T2 MapBase::OpencvPixelToWorld(const T1& point){
//     T2 world_point(
//         origin_.x + (point.x + 0.5) * resolution_, 
//         origin_.y + (height_ - 1 - point.y + 0.5) * resolution_
//     );
//     return world_point;
// }


// 像素坐标转世界坐标
// template <typename T1,  typename T2>
// T2 MapBase::PixelToWorld(const T1& point){
//     T2 world_point(
//         origin_.x + (point.x + 0.5) * resolution_, 
//         origin_.y + (point.y + 0.5) * resolution_
//     );
//     return world_point;
// }

// // 二维数据转一维
// template <typename T>
// size_t MapBase::ToIndex(T pixel_2d){
//     return pixel_2d.y * width_ + pixel_2d.x;
// }


 // 是否有效
// template <typename T>
// bool MapBase::IsValid(T pixel_2d){
//     if(pixel_2d.x < 0 || pixel_2d.x >= width_)
//         return false;
//     if(pixel_2d.y < 0 || pixel_2d.y >= height_)
//         return false;
//     return true;
// }

// bool MapBase::IsValid(size_t idx){
//     if(idx < 0 || idx >= mapSize_)
//         return false;
//     return true;
// }

// 获取像素值，包含边界检测
// template <typename T>
// bool MapBase::GetValue(T pixel_2d, int& value){
//     if(IsValid(pixel_2d)){
//         value = cvMap_.at<uchar>(cv::Point2i(pixel_2d.x, pixel_2d.y));
//         return true;
//     }
//     return false;
// }

// 数值是否有效
// template <typename T>
// bool MapBase::ValueIsValid(T pixel_2d){
//     int value;
//     if(GetValue(pixel_2d, value)){
//         // cout << "value: " << value << endl;
//         if(value < 200) 
//             return false;
//         else
//             return true;
//     }
//     return false;
// }

// 简单膨胀处理
void MapBase::SimpleDilateOccupancyGrid(double robot_radius){
    cv::Mat map_image(height_, width_, CV_8UC1); 
    // for (int i = 0; i < __map_ptr->data.size(); ++i) {
    for (int y = 0; y < height_; ++y) {
        for (int x = 0; x < width_; ++x){
            int index = y * width_ + x;
            if(index < 0 || index >= mapOriginGridPtr_->data.size()) continue;
            int8_t value = mapOriginGridPtr_->data[index];
            uchar pixel_value = static_cast<uchar>(value < 0 ? 128 : (255 - value / 100.0 * 255)); 
            map_image.at<uchar>(y, x) = pixel_value;     
        } 
    }
    cv::Mat map_dilate;
    cv::flip(map_image, map_dilate, 0);    //上下翻转    
    // 障碍物膨胀处理
    int robot_radius_pixel = int(robot_radius / resolution_);
    cv::Mat dilate_struct = cv::getStructuringElement(0, cv::Size(robot_radius_pixel, robot_radius_pixel));     //0 矩形结构元素 1 十字结构元素
    cv::erode(map_dilate, map_dilate, dilate_struct);
    cvMap_ = map_dilate;
}

void MapBase::SimpleDilateOpencvMap(double robot_radius){
    // 障碍物膨胀处理
    int robot_radius_pixel = int(robot_radius / resolution_);
    cv::Mat dilate_struct = cv::getStructuringElement(0, cv::Size(robot_radius_pixel, robot_radius_pixel));     //0 矩形结构元素 1 十字结构元素
    cv::erode(cvMap_, cvMap_, dilate_struct);
}


// 获取地图数据
bool MapBase::getMap(cv::Mat& cv_map){
    if(cvMap_.empty()) return false;
    cv_map = cvMap_.clone();
    return true;
}

// 获取二值地图数据
bool MapBase::getMapBinary(cv::Mat& cv_map){
    if(cvMapBinary_.empty()) return false;
    cv_map = cvMapBinary_.clone();
    return true;
}

bool MapBase::getMap(nav_msgs::OccupancyGrid::Ptr& map_ptr, string frame_id){
    if(cvMap_.empty()) return false;
    map_ptr->data.resize(width_ * height_);
    // cout << "-----------------------------------------" << endl;
    for(int w = 0; w < cvMap_.cols; w++) {
        for(int h = 0; h < cvMap_.rows; h++) {
            if(cvMap_.at<uchar>(h, w) == 0){
                int flipped_h = cvMap_.rows - 1 - h;   //反转y轴坐标 (ros中原点在左下角，opencv中原点在左上角)
                size_t idx = flipped_h * width_ + w;
                if(idx <0 || idx > map_ptr->data.size()) continue;
                map_ptr->data[idx] = 100;
            }       
        }
    }
    map_ptr->info = mapOriginGridPtr_->info;
    map_ptr->header.stamp = ros::Time::now();
    map_ptr->header.frame_id = frame_id;
    return true;
}